% pdf of dij
clear all
% s1mean = [300000e3,300000e3,0];
% s2mean = [0,300000e3,300000e3];
s1mean = [0,0,10000];
s2mean = [0,0,-1000];
s1Sigma= 10*rand(3);
s1Sigma= s1Sigma*s1Sigma';
s2Sigma= rand(3);
s2Sigma= s2Sigma*s2Sigma';

deltamean = s1mean-s2mean;
covDelta  = s1Sigma+s2Sigma;
meanRho   = norm(deltamean);
rhosigma2 = 0;
for i  =1 :3
    for j = 1:3 
        rhosigma2  = rhosigma2 + deltamean(i)*deltamean(j)*covDelta(i,j);
    end
end
rhosigma2  = rhosigma2 / meanRho^2;
disp(meanRho)
disp(sqrt(rhosigma2))

particleAmount = 50000;

d  = zeros(particleAmount,1);

s1s= mvnrnd(s1mean,s1Sigma,particleAmount);
s2s= mvnrnd(s2mean,s2Sigma,particleAmount);


% for i = 1:particleAmount
%     s1 = repmat(s1s(i,:),particleAmount,1);
%     deltaVectors = s1-s2s;
%     norms = arrayfun(@(idx) norm(deltaVectors(idx,:)), 1:size(deltaVectors,1));
%     d(((i-1)*particleAmount+1):(i*particleAmount)) = norms;
% end
deltaVectors = s1s-s2s;
d = arrayfun(@(idx) norm(deltaVectors(idx,:)), 1:size(deltaVectors,1));
disp(mean(d))
disp(std(d))
figure(1)
hist(d,100);
% figure(2);
% scatter3(s1s(:,1),s1s(:,2),s1s(:,3))
% figure(3);
% scatter3(s2s(:,1),s2s(:,2),s2s(:,3))